use std::ops::{Add, AddAssign, Div, Index, IndexMut, Mul, Neg, Sub};

// type Vector<N,T>
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Vector<const N: usize, T>(pub [T; N])
where
    T: Add<T> + Mul<T> + Sub<T> + Div<T> + Neg + PartialOrd + Default + Copy;
pub type Vec2<T> = Vector<2, T>;
pub type Vec3<T> = Vector<3, T>;
pub type Vec4<T> = Vector<4, T>;

pub(crate) type Vec2f = Vector<2, f32>;
pub(crate) type Vec3f = Vector<3, f32>;
pub(crate) type Vec4f = Vector<4, f32>;

// pub type Vec2u = Vector<2, usize>;
// pub type Vec3u = Vector<3, usize>;

pub(crate) type Vec2i = Vector<2, isize>;
pub(crate) type Vec3i = Vector<3, isize>;
pub(crate) type Vec4i = Vector<4, isize>;

// type Matrix<N,T>
#[derive(Clone, Copy, Debug)]
pub struct Matrix<const N: usize, T>(pub [[T; N]; N])
where
    T: Add<T> + Mul<T> + Sub<T> + Div<T> + Neg + PartialOrd + Default + Copy;
pub type Mat2<T> = Matrix<2, T>;
pub type Mat3<T> = Matrix<3, T>;
pub type Mat4<T> = Matrix<4, T>;

pub type Mat2f = Matrix<2, f32>;
pub type Mat3f = Matrix<3, f32>;
pub(crate) type Mat4f = Matrix<4, f32>;

pub type Mat2i = Matrix<2, isize>;
pub type Mat3i = Matrix<3, isize>;
pub type Mat4i = Matrix<4, isize>;
// From
impl<const N: usize, T> From<[T; N]> for Vector<N, T>
where
    T: Add<T> + Mul<T> + Sub<T> + Div<T> + Neg + PartialOrd + Default + Copy,
{
    fn from(data: [T; N]) -> Self {
        Self(data)
    }
}
impl<const N: usize, T> From<[[T; N]; N]> for Matrix<N, T>
where
    T: Add<T> + Mul<T> + Sub<T> + Div<T> + Neg + PartialOrd + Default + Copy,
{
    fn from(data: [[T; N]; N]) -> Self {
        Self(data)
    }
}

// Index and IndexMut
impl<const N: usize, T> Index<usize> for Matrix<N, T>
where
    T: Add<T> + Mul<T> + Sub<T> + Div<T> + Neg + PartialOrd + Default + Copy,
{
    type Output = [T; N];

    fn index(&self, index: usize) -> &Self::Output {
        &self.0[index]
    }
}
impl<const N: usize, T> IndexMut<usize> for Matrix<N, T>
where
    T: Add<T> + Mul<T> + Sub<T> + Div<T> + Neg + PartialOrd + Default + Copy,
{
    fn index_mut(&mut self, index: usize) -> &mut Self::Output {
        &mut self.0[index]
    }
}

impl<const N: usize, T> Index<usize> for Vector<N, T>
where
    T: Add<T> + Mul<T> + Sub<T> + Div<T> + Neg + PartialOrd + Default + Copy,
{
    type Output = T;

    fn index(&self, index: usize) -> &Self::Output {
        &self.0[index]
    }
}
impl<const N: usize, T> IndexMut<usize> for Vector<N, T>
where
    T: Add<T> + Mul<T> + Sub<T> + Div<T> + Neg + PartialOrd + Default + Copy,
{
    fn index_mut(&mut self, index: usize) -> &mut Self::Output {
        &mut self.0[index]
    }
}

// Same Type
impl<const N: usize, T> Add<Self> for Matrix<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Self;
    fn add(self, rhs: Self) -> Self::Output {
        // let m = Self::default();
        let mut m = [[T::default(); N]; N];
        for i in 0..N {
            for j in 0..N {
                m[i][j] = self[i][j] + rhs[i][j];
            }
        }
        Self(m)
        // Self(self.0.map(|line| line.map(|elem| elem + rhs)))
    }
}
// Element wise
impl<const N: usize, T> Add<T> for Matrix<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Self;
    fn add(self, rhs: T) -> Self::Output {
        Self(self.0.map(|line| line.map(|elem| elem + rhs)))
    }
}
impl<const N: usize, T> Sub<T> for Matrix<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Self;
    fn sub(self, rhs: T) -> Self::Output {
        Self(self.0.map(|line| line.map(|elem| elem - rhs)))
    }
}
impl<const N: usize, T> Mul<T> for Matrix<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Self;
    fn mul(self, rhs: T) -> Self::Output {
        Self(self.0.map(|line| line.map(|elem| elem * rhs)))
    }
}
impl<const N: usize, T> Div<T> for Matrix<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Self;
    fn div(self, rhs: T) -> Self::Output {
        Self(self.0.map(|line| line.map(|elem| elem / rhs)))
    }
}

impl<const N: usize, T> Neg for Matrix<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg<Output = T>
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Self;
    fn neg(self) -> Self::Output {
        Self(self.0.map(|line| line.map(|elem| -elem)))
    }
}
impl<const N: usize, T> Add<T> for Vector<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Self;
    fn add(self, rhs: T) -> Self::Output {
        Self(self.0.map(|elem| elem + rhs))
    }
}
impl<const N: usize, T> Add<Self> for Vector<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Self;
    fn add(self, rhs: Self) -> Self::Output {
        let mut m = [T::default(); N];
        for i in 0..N {
            m[i] = self[i] + rhs[i];
        }
        Self(m)
        // Self(self.0.zip(rhs.0).map(|(a, b)| a + b))
    }
}
impl<const N: usize, T> Sub<T> for Vector<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Self;
    fn sub(self, rhs: T) -> Self::Output {
        Self(self.0.map(|elem| elem - rhs))
    }
}

impl<const N: usize, T> Sub<Self> for Vector<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Self;
    fn sub(self, rhs: Self) -> Self::Output {
        let mut m = [T::default(); N];
        for i in 0..N {
            m[i] = self[i] - rhs[i];
        }
        Self(m)
    }
}

impl<const N: usize, T> Mul<T> for Vector<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Self;
    fn mul(self, rhs: T) -> Self::Output {
        Self(self.0.map(|elem| elem * rhs))
    }
}
impl<const N: usize, T> Div<T> for Vector<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Self;
    fn div(self, rhs: T) -> Self::Output {
        Self(self.0.map(|elem| elem / rhs))
    }
}

impl<const N: usize, T> Neg for Vector<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg<Output = T>
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Self;
    fn neg(self) -> Self::Output {
        Self(self.0.map(|elem| -elem))
    }
}
// 用于加权平均
impl<const N: usize, T> Mul<Self> for Vector<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg<Output = T>
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Self;
    fn mul(self, rhs: Self) -> Self::Output {
        let mut data = [T::default(); N];
        for i in 0..N {
            data[i] = self[i] + rhs[i];
        }
        Self(data)
    }
}
// 矩阵乘法
impl<const N: usize, T> Mul<Self> for Matrix<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg<Output = T>
        + AddAssign
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Self;
    fn mul(self, rhs: Self) -> Self::Output {
        let mut data = [[T::default(); N]; N];
        for i in 0..N {
            for k in 0..N {
                for j in 0..N {
                    data[i][j] += self[i][k] * rhs[k][j];
                }
            }
        }
        Self(data)
    }
}
impl<const N: usize, T> Vector<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg<Output = T>
        + PartialOrd
        + Default
        + Copy,
{
    pub fn dot(self, rhs: Self) -> T {
        let mut sum = T::default();
        for i in 0..N {
            sum = (self[i] * rhs[i]) + sum;
        }
        sum
    }
}
// 矩阵乘向量
impl<const N: usize, T> Mul<Vector<N, T>> for Matrix<N, T>
where
    T: Add<Output = T>
        + Mul<Output = T>
        + Sub<Output = T>
        + Div<Output = T>
        + Neg<Output = T>
        + PartialOrd
        + Default
        + Copy,
{
    type Output = Vector<N, T>;
    fn mul(self, rhs: Vector<N, T>) -> Self::Output {
        Vector::from(self.0.map(|line_vec| (Vector::from(line_vec).dot(rhs))))
    }
}

// 特殊的运算，与一些常量，如用到sqrt的normalize，zero与one
impl<const N: usize> Vector<N, f32> {
    pub fn one() -> Self {
        Self([1.0; N])
    }
    pub fn zero() -> Self {
        Self([0.0; N])
    }
    pub fn magnitude(&self) -> f32 {
        self.dot(*self).sqrt()
    }
    pub fn normalize(self) -> Self {
        self / self.magnitude()
    }
}
impl<const N: usize> Vector<N, isize> {
    pub fn one() -> Self {
        Self([1; N])
    }
    pub fn zero() -> Self {
        Self([0; N])
    }
}
// 特殊的运算，与一些常量，如 mvp 矩阵
impl<const N: usize> Matrix<N, f32> {
    pub fn zero() -> Self {
        Self([[0.0; N]; N])
    }
    pub fn identity() -> Self {
        let mut m = Self([[0.0; N]; N]);
        for i in 0..N {
            m[i][i] = 1.0;
        }
        m
    }
    // pub fn model();
}
impl Vec2i {
    pub fn perp_dot(&self, rhs: Self) -> isize {
        self.0[0] * rhs.0[1] - self.0[1] * rhs.0[0]
    }
}
impl Vec2f {
    pub fn perp_dot(&self, rhs: Self) -> f32 {
        self.0[0] * rhs.0[1] - self.0[1] * rhs.0[0]
    }
}
impl Vec4f {
    pub fn truncate(&self) -> Vec3f {
        Vec3f::from([self[0], self[1], self[2]])
    }
}
impl Vec3f {
    pub fn truncate(&self) -> Vec2f {
        Vec2f::from([self[0], self[1]])
    }
    pub fn to_rgb(&self) -> u32 {
        let [r, g, b] = self.0.map(|x| (x as u8) as u32);
        (r << 16) | (g << 8) | b
    }
    pub fn unit_x() -> Self {
        Self([1.0, 0.0, 0.0])
    }
    pub fn unit_y() -> Self {
        Self([0.0, 1.0, 0.0])
    }
    pub fn unit_z() -> Self {
        Self([0.0, 0.0, 1.0])
    }
    pub fn cross(&self, rhs: Self) -> Self {
        Self::from([
            self[1] * rhs[2] - rhs[1] * self[2],
            self[0] * rhs[2] - rhs[0] * self[2],
            self[0] * rhs[1] - rhs[0] * self[1],
        ])
    }
    pub fn to_homo_point(self) -> Vec4f {
        Vec4f::from([self[0], self[1], self[2], 1.0])
    }
    pub fn to_homo_vec(self) -> Vec4f {
        Vec4f::from([self[0], self[1], self[2], 0.0])
    }
}
impl Mat3f {
    pub fn extend(self) -> Mat4f {
        [
            [self[0][0], self[0][1], self[0][2], 0.0],
            [self[1][0], self[1][1], self[1][2], 0.0],
            [self[2][0], self[2][1], self[2][2], 0.0],
            [0.0, 0.0, 0.0, 1.0],
        ]
        .into()
    }
}
// NDC 为右手坐标系，z范围[0,1]
impl Mat4f {
    pub fn transmit(dir: Vec3f) -> Self {
        Self([
            [1.0, 0.0, 0.0, dir[0]],
            [0.0, 1.0, 0.0, dir[1]],
            [0.0, 0.0, 1.0, dir[2]],
            [0.0, 0.0, 0.0, 1.0],
        ])
    }
    pub fn view(eye: Vec3f, dir: Vec3f, vup: Vec3f) -> Self {
        let x_pos = dir.cross(vup).normalize();
        let y_pos = dir.cross(x_pos).normalize();
        let z_pos = -dir.normalize();
        // 正交矩阵 转置 =  求逆
        Self([
            [x_pos[0], y_pos[0], z_pos[0], 0.0],
            [x_pos[1], y_pos[1], z_pos[1], 0.0],
            [x_pos[2], y_pos[2], z_pos[2], 0.0],
            [0.0, 0.0, 0.0, 1.0],
        ]) * Self::transmit(-eye)
        // 先平移 再变换坐标系
    }
    pub fn ortho(t: f32, b: f32, l: f32, r: f32, n: f32, f: f32) -> Self {
        // println!(
        //     "--left right{},{},{}",
        //     -(t + b) / 2.0,
        //     -(l + r) / 2.0,
        //     -(n + f) / 2.0,
        // );
        Self([
            [2.0 / (r - l), 0.0, 0.0, 0.0],
            [0.0, 2.0 / (t - b), 0.0, 0.0],
            [0.0, 0.0, 2.0 / (n - f), 0.0], // 深度值放到正数上来
            [0.0, 0.0, 0.0, 1.0],
        ]) * Self::transmit(Vec3f::from([
            -(l + r) / 2.0,
            -(t + b) / 2.0,
            -(n + f) / 2.0,
        ]))
    }
    pub fn scale(s: f32) -> Self {
        Self([
            [s, 0.0, 0.0, 0.0],
            [0.0, s, 0.0, 0.0],
            [0.0, 0.0, s, 0.0], // 深度值放到正数上来
            [0.0, 0.0, 0.0, 1.0],
        ])
    }
    pub fn perspective(eye_fovy: f32, aspect_ratio: f32, near: f32, far: f32) -> Self {
        assert!(near > 0.0 && far > 0.0, "Near/Far should be positive.");
        // 接口 near far 取正数，面对 -z 运算用负数
        let half_height = (eye_fovy / 2.0).tan() * near; // 弧度制
        let half_width = half_height * aspect_ratio; // ratio = width / height
        let near = -near;
        let far = -far;
        Self::ortho(
            half_height,
            -half_height,
            -half_width,
            half_width,
            near,
            far,
        ) * Self([
            [near, 0.0, 0.0, 0.0],
            [0.0, near, 0.0, 0.0],
            [0.0, 0.0, near + far, -near * far],
            [0.0, 0.0, 1.0, 1.0], // 经过透视投影 相关的点就不再是线性关系了
        ])
    }
    // 罗格里格斯公式
    pub fn rotate(axis: Vec3f, alpha: f32) -> Mat4f {
        let n = axis.normalize(); //.to_homo_vec();
        let n_nt = n.0.map(|elem| (Vec3f::from(n) * elem).0);
        // n_nt[3][3] = 1.0;
        let cos_alpha = alpha.cos();
        // cos_id[3][3] = 1.0;
        let res = Mat3f::identity() * cos_alpha
            + Mat3f::from(n_nt) * (1.0 - cos_alpha)
            + Mat3f::from([[0.0, -n[2], n[1]], [n[2], 0.0, -n[0]], [0.0, -n[2], n[1]]])
                * alpha.sin();
        // res[3][3] = 1.0;
        res.extend()
    }
}
// #[test]
// fn persp() {
//     let v = Vec3f::from([1.6 * 1.732, 1.732, -1.0]);
//     let p = Mat4f::perspective(120.0f32.to_radians(), 1.6, 1.0, 100.0);
//     println!("{:?}", p * v.to_homo_point());
//     assert_eq!((p * v.to_homo_point()), Vec4f::from([1.0, 1.0, -1.0, 1.0]))
// }
